From e5390cad270d49f3b0dc0a03eccfef63ffa7520f Mon Sep 17 00:00:00 2001
From: Naushir Patuck <naush@raspberrypi.com>
Date: Tue, 8 Feb 2022 13:49:11 +0000
Subject: [PATCH] media: i2c: imx219: Scale the pixel clock rate for
 the 640x480 mode

The 640x480 mode uses a special binning mode for high framerate operation where
the pixel rate is effectively doubled. Account for this when setting up the
pixel clock rate, and applying the vblank and exposure controls.

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>
---
 drivers/media/i2c/imx219.c | 33 ++++++++++++++++++++++++++-------
 1 file changed, 26 insertions(+), 7 deletions(-)

--- a/drivers/media/i2c/imx219.c
+++ b/drivers/media/i2c/imx219.c
@@ -164,6 +164,9 @@ struct imx219_mode {
 
 	/* 2x2 binning is used */
 	bool binning;
+
+	/* Relative pixel clock rate factor for the mode. */
+	unsigned int rate_factor;
 };
 
 static const struct imx219_reg imx219_common_regs[] = {
@@ -402,6 +405,7 @@ static const struct imx219_mode supporte
 			.regs = mode_3280x2464_regs,
 		},
 		.binning = false,
+		.rate_factor = 1,
 	},
 	{
 		/* 1080P 30fps cropped */
@@ -419,6 +423,7 @@ static const struct imx219_mode supporte
 			.regs = mode_1920_1080_regs,
 		},
 		.binning = false,
+		.rate_factor = 1,
 	},
 	{
 		/* 2x2 binned 30fps mode */
@@ -436,6 +441,7 @@ static const struct imx219_mode supporte
 			.regs = mode_1640_1232_regs,
 		},
 		.binning = true,
+		.rate_factor = 1,
 	},
 	{
 		/* 640x480 30fps mode */
@@ -453,6 +459,11 @@ static const struct imx219_mode supporte
 			.regs = mode_640_480_regs,
 		},
 		.binning = true,
+		/*
+		 * This mode uses a special 2x2 binning that doubles the
+		 * internal pixel clock rate.
+		 */
+		.rate_factor = 2,
 	},
 };
 
@@ -675,7 +686,8 @@ static int imx219_set_ctrl(struct v4l2_c
 		break;
 	case V4L2_CID_EXPOSURE:
 		ret = imx219_write_reg(imx219, IMX219_REG_EXPOSURE,
-				       IMX219_REG_VALUE_16BIT, ctrl->val);
+				       IMX219_REG_VALUE_16BIT,
+				       ctrl->val / imx219->mode->rate_factor);
 		break;
 	case V4L2_CID_DIGITAL_GAIN:
 		ret = imx219_write_reg(imx219, IMX219_REG_DIGITAL_GAIN,
@@ -695,7 +707,8 @@ static int imx219_set_ctrl(struct v4l2_c
 	case V4L2_CID_VBLANK:
 		ret = imx219_write_reg(imx219, IMX219_REG_VTS,
 				       IMX219_REG_VALUE_16BIT,
-				       imx219->mode->height + ctrl->val);
+				       (imx219->mode->height + ctrl->val) /
+						imx219->mode->rate_factor);
 		break;
 	case V4L2_CID_HBLANK:
 		ret = imx219_write_reg(imx219, IMX219_REG_HTS,
@@ -877,7 +890,7 @@ static int imx219_set_pad_format(struct
 	struct imx219 *imx219 = to_imx219(sd);
 	const struct imx219_mode *mode;
 	struct v4l2_mbus_framefmt *framefmt;
-	int exposure_max, exposure_def, hblank;
+	int exposure_max, exposure_def, hblank, pixel_rate;
 	unsigned int i;
 
 	if (fmt->pad >= NUM_PADS)
@@ -942,6 +955,12 @@ static int imx219_set_pad_format(struct
 						 1,
 						 IMX219_PPL_MIN - mode->width);
 			__v4l2_ctrl_s_ctrl(imx219->hblank, hblank);
+
+			/* Scale the pixel rate based on the mode specific factor */
+			pixel_rate =
+				IMX219_PIXEL_RATE * imx219->mode->rate_factor;
+			__v4l2_ctrl_modify_range(imx219->pixel_rate, pixel_rate,
+						 pixel_rate, 1, pixel_rate);
 		}
 	} else {
 		if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
@@ -1323,7 +1342,7 @@ static int imx219_init_controls(struct i
 	struct v4l2_ctrl_handler *ctrl_hdlr;
 	unsigned int height = imx219->mode->height;
 	struct v4l2_fwnode_device_properties props;
-	int exposure_max, exposure_def, hblank;
+	int exposure_max, exposure_def, hblank, pixel_rate;
 	int i, ret;
 
 	ctrl_hdlr = &imx219->ctrl_handler;
@@ -1335,11 +1354,11 @@ static int imx219_init_controls(struct i
 	ctrl_hdlr->lock = &imx219->mutex;
 
 	/* By default, PIXEL_RATE is read only */
+	pixel_rate = IMX219_PIXEL_RATE * imx219->mode->rate_factor;
 	imx219->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
 					       V4L2_CID_PIXEL_RATE,
-					       IMX219_PIXEL_RATE,
-					       IMX219_PIXEL_RATE, 1,
-					       IMX219_PIXEL_RATE);
+					       pixel_rate, pixel_rate,
+					       1, pixel_rate);
 
 	imx219->link_freq =
 		v4l2_ctrl_new_int_menu(ctrl_hdlr, &imx219_ctrl_ops,
